#include "StdAfx.h"
#include <I3DEngine.h>
#include <IRenderAuxGeom.h>
#include "../CharacterInstance.h"
#include "../Model.h"
#include "../ModelSkeleton.h"
#include "../CharacterManager.h"

#include "PoseModifierHelper.h"
#include "PoseBlenderAim.h"

#include <CryExtension/CryCreateClassInstance.h>

CPoseBlenderAim& CSkeletonPose::GetPoseBlenderAim() 
{ 
	CPoseBlenderAim *pbAim = static_cast<CPoseBlenderAim*>(m_PoseBlenderAim.get());

	if (pbAim == NULL)
	{
		::CryCreateClassInstance<IAnimationPoseBlenderDir>("AnimationPoseModifier_PoseBlenderAim", m_PoseBlenderAim);
		pbAim = static_cast<CPoseBlenderAim*>(m_PoseBlenderAim.get());
		assert(pbAim);
		pbAim->m_blender.Init();
	}

	return *pbAim; 
}

//--------------------------------------------------------------------------------------
//--------                enables/disables aim IK and sets aim target			--------
//--------------------------------------------------------------------------------------
void CSkeletonPose::SetAimIK(bool useAimIK, const Vec3& vGlobalAimIKTarget, float blendTime) 
{
	CPoseBlenderAim &poseBlenderAim = GetPoseBlenderAim();

	poseBlenderAim.m_blender.m_Set_UseDirIK = useAimIK;

	if (blendTime != -1.0f)
	{
		poseBlenderAim.m_blender.m_Set_fDirIKBlend = blendTime;
	}

	//	m_AimIK().m_Set_AimIKBlend=0.5f;

	if (useAimIK && !vGlobalAimIKTarget.IsValid())
	{
#if !defined(__SPU__)
		g_pISystem->Warning(VALIDATOR_MODULE_ANIMATION, VALIDATOR_WARNING, VALIDATOR_FLAG_FILE, m_pInstance->GetFilePath(), 
			"CryAnimation: aimIKTarget Invalid (%f %f %f)", vGlobalAimIKTarget.x, vGlobalAimIKTarget.y, vGlobalAimIKTarget.z);
#ifdef _DEBUG
		g_pISystem->debug_LogCallStack();
#endif
#endif
	}
	else
	{
		poseBlenderAim.m_blender.m_Set_vDirIKTarget = vGlobalAimIKTarget;
	}
}


void CSkeletonPose::SetPoseBlenderAimState(bool bEnable)
{
	GetPoseBlenderAim().m_blender.m_Set_UseDirIK = bEnable;
}

void CSkeletonPose::SetPoseBlenderAimLayer(uint32 layer)
{
	CPoseBlenderAim &blenderAim = GetPoseBlenderAim();
	blenderAim.m_blender.m_Set_nLayer = max(layer, 1u);
}








//

CRYREGISTER_CLASS(CPoseBlenderAim)
CPoseBlenderAim::CPoseBlenderAim()
{
}


CPoseBlenderAim::~CPoseBlenderAim()
{
}


//
bool CPoseBlenderAim::Prepare(const SAnimationPoseModiferParams& params)
{
	//	float fTextColor[4] = {1,0,0,1};
	//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"Prepare Aim-IK" );	
	//	g_YLine+=16.0f;

	m_blender.m_UseDirIK							= m_blender.m_Set_UseDirIK;
	m_blender.m_nDirLayer							= m_blender.m_Set_nLayer;
	m_blender.m_vDirIKTarget						= m_blender.m_Set_vDirIKTarget;
	m_blender.m_fDirIKTargetSmoothTime = m_blender.m_Set_fDirIKTargetSmoothTime;
	m_blender.m_fDirIKBlend						= m_blender.m_Set_fDirIKBlend;
	m_blender.m_fDirIKFadeInTime			= m_blender.m_Set_fDirIKFadeInTime;
	m_blender.m_fDirIKFadeOutTime			= m_blender.m_Set_fDirIKFadeOutTime;
	m_blender.m_PolarCoordinates			= m_blender.m_Set_PolarCoordinates;
	return true;
}

SPU_NO_INLINE bool CPoseBlenderAim::Update(const SAnimationPoseModiferParams& params)
{
	//	float fTextColor[4] = {1,0,0,1};
	//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"Update Aim-IK" );	
	//	g_YLine+=16.0f;
	m_blender.m_fDirIKInfluence = 0;

	if (Console::GetInst().ca_UseAimIK == 0)
		return false;
	CCharInstance* pInstance = PoseModifierHelper::GetCharInstance(params);
	if (pInstance->m_pModel->m_ObjectType==CGA)
		return false;  //error-check: we shouldn't execute the PM any more

	SmoothCD(m_blender.m_vDirIKTargetSmooth, m_blender.m_vDirIKTargetRate, params.timeDelta, m_blender.m_vDirIKTarget, m_blender.m_fDirIKTargetSmoothTime);

	uint32 useAimIK = (m_blender.m_Set_UseDirIK || m_blender.m_Set_fDirIKBlend);
	if (useAimIK==0)
		return false;

	CModelSkeleton* pModelSkeleton = PoseModifierHelper::GetModelSkeleton(params);
	uint32 numDirBlends = pModelSkeleton->m_AimDirBlends.size();
	if (numDirBlends==0)
		return false;  //error-check: we shouldn't execute the PM any more
	for (uint32 d=0; d<numDirBlends; d++)
	{
		if (pModelSkeleton->m_AimDirBlends[d].m_nParaJointIdx<0 || pModelSkeleton->m_AimDirBlends[d].m_nStartJointIdx<0 || pModelSkeleton->m_AimDirBlends[d].m_nReferenceJointIdx<0)
		{
#if !defined(__SPU__)				
			const char* pModelName = pInstance->m_pModel->GetFilePath();
			g_pISystem->Warning(VALIDATOR_MODULE_ANIMATION, VALIDATOR_WARNING, VALIDATOR_FLAG_FILE, pInstance->GetFilePath(), "CryAnimation: No weapon-bone specified for model: %s",pModelName );
#endif		
			return false;  //error-check: we shouldn't execute the PM any more
		}
	}
	uint32 numRotJoints = pModelSkeleton->m_AimIK_Rot.size();
	if (numRotJoints==0)
	{
#if !defined(__SPU__)
		const char* pModelName = pInstance->m_pModel->GetFilePath();
		g_pISystem->Warning(VALIDATOR_MODULE_ANIMATION, VALIDATOR_WARNING, VALIDATOR_FLAG_FILE, pInstance->GetFilePath(), "CryAnimation: No AimIK-Setup for model: %s",pModelName );
#endif
		return false;   //error-check: we shouldn't execute the PM any more
	}

	// Pose Blender Aim initialization
	// TODO: Move this as part to the PoseModifier prepare stage.
	CSkeletonPose* pSkeletonPose = PoseModifierHelper::GetSkeletonPose(params);
	CSkeletonAnim* pSkeletonAnim = PoseModifierHelper::GetSkeletonAnim(params);

	if (pSkeletonPose->m_timeStandingUp >= 0)
		return false;

	pSkeletonAnim->Commands_CreateAimPoseQueue(m_blender, 1);  //here we should call Prepare(), and then the rest can be inside
	if (m_blender.m_numActiveDirPoses==0)
		return false; // no animation in base-layer, no aim-pose

	if (pModelSkeleton->m_AimDirBlends.size())
	{
		int32 nWeaponBone = pModelSkeleton->m_AimDirBlends[0].m_nParaJointIdx;
		if (nWeaponBone>0)
		{
#if !defined(__SPU__)
			if (m_blender.m_Set_UseDirIK && Console::GetInst().ca_DrawAimPoses)
			{
				g_pAuxGeom->SetRenderFlags( e_Def3DPublicRenderflags );
				static Ang3 angle1(0,0,0); 
				angle1 += Ang3(0.01f,0.02f,0.08f);
				AABB aabb = AABB(Vec3(-0.09f,-0.09f,-0.09f),Vec3(+0.09f,+0.09f,+0.09f));
				OBB obb1=OBB::CreateOBBfromAABB( Matrix33::CreateRotationXYZ(angle1),aabb );
				g_pAuxGeom->DrawOBB(obb1, m_blender.m_vDirIKTargetSmooth,1,RGBA8(0x00,0x00,0xff,0xff), eBBD_Extremes_Color_Encoded);
			}
#endif
			f32 fFrameTime = params.timeDelta;
			if (fFrameTime<0.0f) fFrameTime=0.0f;
			if (m_blender.m_nDirIKFadeout || m_blender.m_Set_UseDirIK==0) 
				m_blender.m_Set_fDirIKBlend -= div_min(fFrameTime, m_blender.m_Set_fDirIKFadeOutTime, m_blender.m_Set_fDirIKBlend);
			else
				m_blender.m_Set_fDirIKBlend += div_min(fFrameTime, m_blender.m_Set_fDirIKFadeInTime, 1.0f - m_blender.m_Set_fDirIKBlend);
		}
	}

	m_blender.m_nDirIKFadeout = 0;
	if (m_blender.m_fDirIKBlend < 0.001f)
		return false;  //IK not visible: we shouldn't execute the PM any more

	return true;
}

SPU_INDIRECT(CommandBufferExecute(ML))
bool CPoseBlenderAim::Execute(const SAnimationPoseModiferParams& params) 
{
	//	float fTextColor[4] = {1,0,0,1};
	//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"Execute Aim-IK" );	
	//	g_YLine+=16.0f;

	CModelSkeleton* pModelSkeleton	= PoseModifierHelper::GetModelSkeleton(params);
	CSkeletonPose* pSkeletonPose		= PoseModifierHelper::GetSkeletonPose(params);

	uint32 numDB=pModelSkeleton->m_AimDirBlends.size();
	uint32 numRotJoints = pModelSkeleton->m_AimIK_Rot.size();
	uint32 numPosJoints = pModelSkeleton->m_AimIK_Pos.size();
	SpuStackValue<SDirectionalBlender,true,true> rBlender(m_blender);

	SJointsAimIK_Pos* pPos=0;
	if (numPosJoints)
		pPos=&pModelSkeleton->m_AimIK_Pos[0];

	rBlender->ExecuteDirectionalIK(params, &pModelSkeleton->m_AimDirBlends[0],numDB, &pModelSkeleton->m_AimIK_Rot[0],numRotJoints, pPos,numPosJoints,RGBA8(0xff,0xff,0xff,0x00));

	//procedural adjustments
	CModelJoint* parrModelJoints = &pModelSkeleton->m_arrModelJoints[0];
	QuatT* const __restrict pRelPose = SPU_LOCAL_PTR(&params.pPoseRelative[0]);
	QuatT* const __restrict pAbsPose = SPU_LOCAL_PTR(&params.pPoseAbsolute[0]);

	const QuatTS& rAnimLocationNext = SPU_LOCAL_REF(params.locationNextAnimation);
	Vec3 vLocalAimIKTarget=(rBlender->m_vDirIKTargetSmooth-rAnimLocationNext.t)*rAnimLocationNext.q;
	for (uint32 g=0; g<numDB; g++)
	{
		uint32 numProc = pModelSkeleton->m_AimProcAdjustments.size();
		for (uint32 i=0; i<numProc; i++)
		{
			if (pModelSkeleton->m_AimProcAdjustments[i].m_nIdx<0)
				numProc=0;
		}
		int32 nDirJointIdx = rBlender->m_dbw[g].m_nParaJointIdx;

		f32 wR = rBlender->m_dbw[g].m_fWeight;
		f32 fInfluence = rBlender->m_fDirIKInfluence * rBlender->m_fDirIKInfluence * rBlender->m_fDirIKInfluence * rBlender->m_fDirIKInfluence*wR;
		if (nDirJointIdx>0 && numProc && fInfluence)
		{
			f32 fDistribution=1.0f/f32(numProc);
			for (uint32 d=0; d<4; d++)
			{
				Vec3 realdir	=	pAbsPose[nDirJointIdx].q.GetColumn1();
				Vec3 wantdir	=	(vLocalAimIKTarget-pAbsPose[nDirJointIdx].t).GetNormalized();
				f32 angle = acos_tpl( realdir|wantdir );
				if (angle<0.001f)
					continue;

				f32 blend = 1-MIN(fabsf(angle),1.0f);

				//	float fColDebug[4] = {1,1,0,1};
				//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.3f, fColDebug, false,"angle: %f   blend: %f  fDistribution: %f wR: %f",angle,blend,fDistribution,wR );	
				// g_YLine+=16.0f;

				Quat rel; rel.SetRotationV0V1(realdir,wantdir);
				rel.SetNlerp(IDENTITY,rel,blend*fDistribution);

				for (uint32 numSJ=1; numSJ<numProc; numSJ++)
				{
					int32 nSpine2 = pModelSkeleton->m_AimProcAdjustments[numSJ-1].m_nIdx;
					int32 nSpine3 = pModelSkeleton->m_AimProcAdjustments[numSJ].m_nIdx;
					pAbsPose[nSpine3].q.SetNlerp(pAbsPose[nSpine3].q,rel*pAbsPose[nSpine3].q,fInfluence);
					pRelPose[nSpine3].q = !pAbsPose[nSpine2].q*pAbsPose[nSpine3].q;
				}

				for (uint32 r=0; r<numRotJoints; r++)
				{
					int32 j	= pModelSkeleton->m_AimIK_Rot[r].m_nJointIdx;
					int32 p = parrModelJoints[j].m_idxParent;
					pAbsPose[j]	= pAbsPose[p] * pRelPose[j];
					pAbsPose[j].q.Normalize();
				}
			}
		}
	}

#ifdef _DEBUG
	for (uint32 j=0; j<params.jointCount; j++)
	{
		assert( pRelPose[j].q.IsUnit() );
		assert( pAbsPose[j].q.IsUnit() );
		assert( pRelPose[j].IsValid() );
		assert( pAbsPose[j].IsValid() );
	}
#endif			

	return 1;
}

void CPoseBlenderAim::Synchronize()
{
}













//-----------------------------------------------------------------------------------------------------
//-----                  compute pose for AIM-IK and for Look-IK                            -----------
//-----------------------------------------------------------------------------------------------------
bool SDirectionalBlender::ExecuteDirectionalIK(const SAnimationPoseModiferParams& params,DirectionalBlends* rDirBlends,uint32 numDB, SJointsAimIK_Rot* rRot,uint32 numRotJoints, SJointsAimIK_Pos* rPos,uint32 numPosJoints, const ColorB& col )
{
	float fColDebug[4] = {1,1,0,1};

	CModelSkeleton* pModelSkeleton	= PoseModifierHelper::GetModelSkeleton(params);
	CModelJoint* parrModelJoints		= &pModelSkeleton->m_arrModelJoints[0];
	QuatT* const __restrict pRelPose = SPU_LOCAL_PTR(&params.pPoseRelative[0]);
	QuatT* const __restrict pAbsPose = SPU_LOCAL_PTR(&params.pPoseAbsolute[0]);

	const QuatTS& rAnimLocationNext = SPU_LOCAL_REF(params.locationNextAnimation);
	Vec3 vLocalAimIKTarget=(m_vDirIKTargetSmooth-rAnimLocationNext.t)*rAnimLocationNext.q;
	uint32 numJoints = params.jointCount;
	pAbsPose[0] = pRelPose[0];
	for (uint32 i=1; i<numJoints; i++)
		pAbsPose[i]	= pAbsPose[pModelSkeleton->GetJointParentIDByID(i)] * pRelPose[i];

	//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.6f, fColDebug, false,"rAnimLocationNext: %f (%f %f %f)",rAnimLocationNext.q.w,rAnimLocationNext.q.v.x,rAnimLocationNext.q.v.y,rAnimLocationNext.q.v.z );	
	//	g_YLine+=16.0f;


	QuatT wRefJoint = QuatT(rAnimLocationNext)*pAbsPose[rDirBlends->m_nReferenceJointIdx];
	//	QuatT wRefJoint = QuatT(rAnimLocationNext)*pAbsPose[1];
	Vec3 fw=wRefJoint.q.GetColumn1();
	Vec3 up=(rDirBlends->m_nReferenceJointIdx) ? wRefJoint.q.GetColumn0() : wRefJoint.q.GetColumn2();
	Vec3 si=up%fw;

	Matrix33 p33 = Matrix33(wRefJoint.q);
	Plane LR_Plane; LR_Plane.SetPlane(up%fw,wRefJoint.t);
	Plane FB_Plane; FB_Plane.SetPlane(si%up,wRefJoint.t);
	Plane UD_Plane; UD_Plane.SetPlane(fw%si,wRefJoint.t);
	uint32 BR = isneg(LR_Plane|m_vDirIKTargetSmooth);
	uint32 FB = isneg(FB_Plane|m_vDirIKTargetSmooth);
	float  UD = UD_Plane|m_vDirIKTargetSmooth;
	Vec3   OnPlane=m_vDirIKTargetSmooth-UD*UD_Plane.n;
	f32		 CosineY=(OnPlane-wRefJoint.t).GetNormalizedSafe(Vec3(0,1,0))|fw;

	/*
	g_pAuxGeom->DrawLine(	wRefJoint.t,RGBA8(0x7f,0x00,0x00,0xff), wRefJoint.t+up*10,RGBA8(0xff,0x00,0x00,0xff) );
	g_pAuxGeom->DrawLine(	wRefJoint.t,RGBA8(0x00,0x7f,0x00,0xff), wRefJoint.t+fw*10,RGBA8(0x00,0xff,0x00,0xff) );
	g_pAuxGeom->DrawLine(	wRefJoint.t,RGBA8(0x00,0x00,0x7f,0xff), wRefJoint.t+si*10,RGBA8(0x00,0x00,0xff,0xff) );


	g_pIRenderer->Draw2dLabel( 1,g_YLine, 2.4f, fColDebug, false,"BR: %f   UD: %f   OnPlane: %f %f %f   CosineY: %f",(LR_Plane|m_vDirIKTargetSmooth),UD,OnPlane.x,OnPlane.y,OnPlane.z, CosineY );	
	g_YLine+=26.0f;

	if (BR==0)
	{
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 2.4f, fColDebug, false,"Left Side");	
	g_YLine+=26.0f;
	}
	if (BR==1)
	{
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 2.4f, fColDebug, false,"Right Side");	
	g_YLine+=26.0f;
	}

	if (FB==0)
	{
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 2.4f, fColDebug, false,"Front Side");	
	g_YLine+=26.0f;
	}
	if (FB==1)
	{
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 2.4f, fColDebug, false,"Back Side");	
	g_YLine+=26.0f;
	}
	*/

	//-----------------------------------------------------------------------------------------------

	//Some rules to disable Aim-IK. Target is too close to the weapon
	//	Vec3 _nWeaponBoneWorld	= rAnimLocationNext*pAbsPose[m_dbw[0].m_nParaJointIdx].t;
	//	f32 fDistance = (vLocalAimIKTarget-_nWeaponBoneWorld).GetLength();
	//	if ( fDistance<1.0f )
	//		m_nDirIKFadeout=1;   //trigger fade-out if too close

	//------------------------------------------------------------------------------------
	//---------         here we know that we can actually use AimIK          -------------
	//------------------------------------------------------------------------------------
	for (uint32 d=0; d<numDB; d++)
	{
		m_dbw[d].m_nAnimTokenCRC32	= rDirBlends[d].m_AnimTokenCRC32;
		m_dbw[d].m_nParaJointIdx		=	rDirBlends[d].m_nParaJointIdx;
		m_dbw[d].m_nStartJointIdx		=	rDirBlends[d].m_nStartJointIdx;
		m_dbw[d].m_fWeight					=	0.0f;
	}

	//	for (uint32 z=0; z<1; z++)
	//	{

	f32 fWeightOfAllAimPoses=0;
	for (int32 i=0; i<m_numActiveDirPoses; i++)
			fWeightOfAllAimPoses += m_DirInfo[i].m_fWeight;

	QuatT arrRelPose[64];
	f32 fIKBlend = clamp(m_fDirIKBlend,0.0f,1.0f)-0.5f;
	fIKBlend = fIKBlend/(0.5f+2.0f*fIKBlend*fIKBlend)+0.5f;
	m_fDirIKInfluence = fIKBlend*fWeightOfAllAimPoses;
	f32 t0 = 1.0f-m_fDirIKInfluence;
	assert(t0< 1.001f);
	assert(t0>-0.001f);

	// 	g_YLine+=16.0f;
	//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.6f, fColDebug, false,"m_numActiveDirPoses: %d  fWeightOfAllAimPoses: %f  fIKBlend: %f   t0: %f",m_numActiveDirPoses,fWeightOfAllAimPoses,fIKBlend,t0 );	
	//	g_YLine+=16.0f;

	f32 t1 = fIKBlend;
	SJointsAimIK_Rot* pAimIK_Rot = &rRot[0];
	for (uint32 r=0; r<numRotJoints; r++)
	{
		int32 j	=pAimIK_Rot[r].m_nJointIdx;
		arrRelPose[r].q.w	 =pRelPose[j].q.w;		pRelPose[j].q.w		*= t0;
		arrRelPose[r].q.v.x=pRelPose[j].q.v.x;	pRelPose[j].q.v.x *= t0;
		arrRelPose[r].q.v.y=pRelPose[j].q.v.y;	pRelPose[j].q.v.y *= t0;
		arrRelPose[r].q.v.z=pRelPose[j].q.v.z;	pRelPose[j].q.v.z *= t0;
		arrRelPose[r].t.x = pRelPose[j].t.x;		pRelPose[j].t.x		*= t0;
		arrRelPose[r].t.y = pRelPose[j].t.y;		pRelPose[j].t.y		*= t0;
		arrRelPose[r].t.z = pRelPose[j].t.z;		pRelPose[j].t.z		*= t0;
	}



	assert(m_numActiveDirPoses);

	f32 fTotalWeight = t0;
	GlobalAnimationHeaderAIM* parrGlobalAIM = &g_AnimationManager.m_arrGlobalAIM[0];

	for (int32 a=0; a<m_numActiveDirPoses; a++)
	{
		uint32 numAimPosesPerAnim = m_DirInfo[a].m_numDirPoses;
		assert(numAimPosesPerAnim==1 || numAimPosesPerAnim==2);
		if (numAimPosesPerAnim==1)
		{	
			assert(m_DirInfo[a].m_nGlobalDirID0>=0);
			GlobalAnimationHeaderAIM& rAIM = parrGlobalAIM[m_DirInfo[a].m_nGlobalDirID0];
			uint32 nAnimTokenCRC32 = rAIM.m_AnimTokenCRC32;
			f32 fAimPoseWeight = m_DirInfo[a].m_fWeight;
			for (uint32 d=0; d<numDB; d++)
			{
				if (m_dbw[d].m_nAnimTokenCRC32==rAIM.m_AnimTokenCRC32)
				{
					m_dbw[d].m_fWeight += t1*fAimPoseWeight;
					AccumulateAimPoses( params,rAIM,arrRelPose,t1*fAimPoseWeight,vLocalAimIKTarget,m_dbw[d].m_nParaJointIdx,m_dbw[d].m_nStartJointIdx,rRot,numRotJoints, rPos,CosineY,(BR|FB<<1),col);
					fTotalWeight += t1*fAimPoseWeight;
#if !(defined(XENON) || defined(PS3))
					if (Console::GetInst().ca_DrawAimIKVEGrid) DebugVEGrid( params, m_DirInfo[a],fAimPoseWeight,fIKBlend,m_dbw[d].m_nParaJointIdx,rRot,numRotJoints, rPos,numPosJoints);
#endif
					break;
				}
			}
			continue;
		}

		if (numAimPosesPerAnim==2)
		{	
			assert(m_DirInfo[a].m_nGlobalDirID0>=0);
			GlobalAnimationHeaderAIM& rAIM0 = parrGlobalAIM[m_DirInfo[a].m_nGlobalDirID0];
			f32 a0 = 1.0f-m_DirInfo[a].m_fAnimTime;
			f32 fAimPoseWeight0 = m_DirInfo[a].m_fWeight*a0;
			for (uint32 d=0; d<numDB; d++)
			{
				if (m_dbw[d].m_nAnimTokenCRC32==rAIM0.m_AnimTokenCRC32)
				{
					m_dbw[d].m_fWeight += t1*fAimPoseWeight0;
					AccumulateAimPoses( params,rAIM0,arrRelPose,t1*fAimPoseWeight0,vLocalAimIKTarget,m_dbw[d].m_nParaJointIdx,m_dbw[d].m_nStartJointIdx,rRot,numRotJoints, rPos,CosineY,(BR|FB<<1),col);
					fTotalWeight += t1*fAimPoseWeight0;
					break;
				}
			}

			assert(m_DirInfo[a].m_nGlobalDirID1>=0);
			GlobalAnimationHeaderAIM& rAIM1 = parrGlobalAIM[m_DirInfo[a].m_nGlobalDirID1];
			f32 a1 = m_DirInfo[a].m_fAnimTime;
			f32 fAimPoseWeight1 = m_DirInfo[a].m_fWeight*a1;
			for (uint32 d=0; d<numDB; d++)
			{
				if (m_dbw[d].m_nAnimTokenCRC32==rAIM1.m_AnimTokenCRC32)
				{
					m_dbw[d].m_fWeight += t1*fAimPoseWeight1;
					AccumulateAimPoses( params,rAIM1,arrRelPose,t1*fAimPoseWeight1,vLocalAimIKTarget,m_dbw[d].m_nParaJointIdx,m_dbw[d].m_nStartJointIdx,rRot,numRotJoints, rPos,CosineY,(BR|FB<<1), col);
					fTotalWeight += t1*fAimPoseWeight1;
					break;
				}
			}
			continue;
		}
		assert(0);
	}
	assert( fabsf(fTotalWeight-1.0f) < 0.001f );


	for (uint32 r=0; r<numRotJoints; r++)
	{
		int32 j	= pAimIK_Rot[r].m_nJointIdx;
		int32 p = parrModelJoints[j].m_idxParent;
		pRelPose[j].q.Normalize();
		pAbsPose[j]	= pAbsPose[p] * pRelPose[j];
	}

	//	}




#if !(defined(XENON) || defined(PS3))
	for (uint32 g=0; g<numDB; g++)
	{
		int32 nDirJointIdx = m_dbw[g].m_nParaJointIdx;
		f32 wR = m_dbw[g].m_fWeight;

		float fTextColor[4] = {1,0,0,1};
		if( Console::GetInst().ca_DrawAimPoses && wR>0.01f)
		{
			Vec3 RWBone		= rAnimLocationNext.q*pAbsPose[nDirJointIdx].t;
			Vec3 WStart		= rAnimLocationNext.t+RWBone;
			Vec3 realdir	=	pAbsPose[nDirJointIdx].q.GetColumn1();
			Vec3 WEnd   = WStart + rAnimLocationNext.q*realdir*10;

			g_pAuxGeom->DrawLine(	WStart,RGBA8(0x00,0x7f,0x7f,0xff), WEnd,RGBA8(0x00,0xff,0xff,0xff) );
			GlobalAnimationHeaderAIM rAIM;
			Vec2 vPolarCoord = Vec2( rAIM.Debug_PolarCoordinate(pAbsPose[nDirJointIdx].q) );
			g_YLine+=16.0f;
			g_pIRenderer->Draw2dLabel( 1,g_YLine, 2.4f, fTextColor, false,"final polar: %f %f   wR: %f",vPolarCoord.x,vPolarCoord.y,wR );	
			g_YLine+=16.0f;
		}
		//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"Aim-IK Influence: %f %f %f",m_fDirIKInfluence,fTotalWeight,fIKBlend );	
		//	g_YLine+=16.0f;
	}
#endif

	if (CosineY < 0.0f)
		return 0;

	return true;
}


SPU_NO_INLINE void SDirectionalBlender::AccumulateAimPoses(const SAnimationPoseModiferParams& params, GlobalAnimationHeaderAIM& rAim, QuatT* arrRelPose,f32 fIKBlend, const Vec3& vLocalAimIKTarget, int32 widx,int32 nStartJointIdx,  SJointsAimIK_Rot* rRot,uint32 numRotJoints,  SJointsAimIK_Pos* rPos,f32 CosineY,uint32 sides, const ColorB& col )
{
	CCharInstance* pInstance = PoseModifierHelper::GetCharInstance(params);
	SpuStackValue<GlobalAnimationHeaderAIM,true,true> stackGAH(rAim);
	GlobalAnimationHeaderAIM &rAIM = stackGAH;

	Matrix33 m33; m33.SetRotationX(-gf_PI/2);
	m33.m00=-m33.m00;	
	m33.m01=-m33.m01; 
	m33.m02=-m33.m02;

	uint32 numAimPosesCAF = rAIM.m_arrAimIKPosesAIM.size();	
	assert(numAimPosesCAF==9 || numAimPosesCAF==15 || numAimPosesCAF==27);
	if (numAimPosesCAF==0)
		return;

	uint32 AimPoseMid = uint32(numAimPosesCAF/2);

	CModelSkeleton* pModelSkeleton	=	&pInstance->m_pModel->m_ModelSkeleton;
	CModelJoint* parrModelJoints		= &pModelSkeleton->m_arrModelJoints[0];

	////////////////////////////////////////////////////////////////////////////

	float fTextColor[4] = {1,0,0,1};

	QuatT qtemp;
	DynArray<Vec3> & rAimMidPosistions = rAIM.m_arrAimIKPosesAIM[AimPoseMid].m_arrPosition;
	const Quat*  const __restrict pMiddleRot = &rAIM.m_arrAimIKPosesAIM[AimPoseMid].m_arrRotation[0];
	const Vec3*  __restrict pMiddlePos = 0;
	if (!rAimMidPosistions.empty())
		pMiddlePos = &rAimMidPosistions[0];

	QuatT* const __restrict jointsRelative = SPU_PTR_SELECT(&params.pPoseRelative[0], gSkeletonRelativePose);	
	QuatT* const __restrict jointsAbsolute = SPU_PTR_SELECT(&params.pPoseAbsolute[0], gSkeletonAbsolutePose);	

	//g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"rAIM.m_nExist: %016x",rAIM.m_nExist);	
	//g_YLine+=16.0f;

	for (uint32 r=0; r<numRotJoints; r++)
	{		
		if (rRot[r].m_nPreEvaluate)
		{
			int32 j	= rRot[r].m_nJointIdx;
			int32 pidx = parrModelJoints[j].m_idxParent;
			qtemp = arrRelPose[r];
			if (rAIM.m_nExist & (uint64(1)<<r))
			{
				//	CModelSkeleton* pModelSkeleton	= PoseModifierHelper::GetModelSkeleton(params);
				//	const char* pjointname = pModelSkeleton->GetJointNameByID(j);
				//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"rot joint: %d %s",j,pjointname);	
				//	g_YLine+=16.0f;

				qtemp.q = pMiddleRot[r];
				assert(qtemp.q.IsValid());
				assert(pMiddleRot[r].IsValid());
				if (rRot[r].m_nAdditive) 
					qtemp.q	= arrRelPose[r].q*(!pMiddleRot[r]*qtemp.q);     //additive blend
				int32 p	= rRot[r].m_nPosIndex;
				if (p>-1 && pMiddlePos)
				{
					qtemp.t = pMiddlePos[p];
					if (rPos[p].m_nAdditive) 
						qtemp.t	= arrRelPose[r].t - (pMiddlePos[r]-qtemp.t);     //additive blend
				}
			}
			jointsAbsolute[j]	= jointsAbsolute[pidx] * qtemp;
		}
	}
	Quat qMiddle = jointsAbsolute[widx].q*rAIM.m_MiddleAimPoseRot;

	//	QuatT qMiddle2 = jointsAbsolute[widx]*rAIM.m_MiddleAimPoseRot;
	//	g_pAuxGeom->DrawLine( qMiddle2.t,col, qMiddle2.t+qMiddle2.GetColumn1()*10.0f,col );
	//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"qMiddle: %f (%f %f %f)",qMiddle.w,  qMiddle.v.x,qMiddle.v.y,qMiddle.v.z );	
	//	g_YLine+=16.0f;

	Vec3 vRefPose=Vec3(0,0,1.6f);
	if (nStartJointIdx>-1)
		vRefPose=jointsAbsolute[nStartJointIdx].t;


	Vec3 vAimVec = (vLocalAimIKTarget-vRefPose).GetNormalized()*qMiddle;
	Vec2 p;
	p.x = atan2f(-vAimVec.x,vAimVec.y);
	f32 s1,c1; sincos_tpl(p.x,&s1,&c1);
	p.y = -atan2f(vAimVec.z,-vAimVec.x*s1+vAimVec.y*c1);
	p.x=clamp(p.x,-gf_PI+0.01f,gf_PI-0.01f);


	if (sides&2)
	{
		//we are behind the character
		//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 2.4f, fTextColor, false,"Behind the Character");	
		g_YLine+=26.0f;
		if ((sides&1)==0 && p.x<0.0f)
			p.x=+gf_PI-0.01f;
		if ((sides&1)==1 && p.x>0.0f)
			p.x=-gf_PI+0.01f;
	}
	if (p.y<-1.40f) p.y=-1.40f;
	if (p.y> 1.40f) p.y= 1.40f;

	//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"p: %f %f",p.x,p.y );	
	//	g_YLine+=16.0f;

	f32 st = (1-CosineY);
	st = st*st*0.08f+0.02f;
	SmoothCD(m_PolarCoordinatesSmooth, m_PolarCoordinatesRate, g_AverageFrameTime, p, st);
	//g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"st: %f",st);	
	//g_YLine+=16.0f;

	//g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"m_PolarCoordinatesSmooth: %f %f",m_PolarCoordinatesSmooth.x,m_PolarCoordinatesSmooth.y );	
	//g_YLine+=16.0f;


#if !(defined(XENON) || defined(PS3))
	if (Console::GetInst().ca_DrawAimIKVEGrid)
	{
		static Ang3 ang3 = Ang3(0,0,0);
		ang3+=Ang3(0.01f,0.02f,0.13f);
		AABB aabb=AABB( Vec3(-0.02f,-0.02f,-0.02f), Vec3(+0.02f,+0.02f,+0.02f));
		OBB obb; obb.SetOBBfromAABB(Matrix33::CreateRotationXYZ(ang3),aabb);
		g_pAuxGeom->DrawOBB(obb,m33*Vec3(m_PolarCoordinatesSmooth),1,col,eBBD_Extremes_Color_Encoded);
	}
#endif


	uint8 idx,i0,i1,i2,i3;
	f32 fx=(m_PolarCoordinatesSmooth.x+gf_PI  )/(gf_PI/8);
	f32 fy=(m_PolarCoordinatesSmooth.y+gf_PI/2)/(gf_PI/8);
	int32 ix=int32(fx);
	int32 iy=int32(fy);
	f32 px=fx-f32(ix);
	f32 py=fy-f32(iy);

	if (ix<0)	 ix=0;
	if (ix>15) ix=15;
	if (iy<0)	 iy=0;
	if (iy>7)	 iy=7;

	f32 weights[256];
	for (uint32 i=0; i<numAimPosesCAF; i++) weights[i]=0;

	CHUNK_GAHAIM_INFO::VirtualExample *pPolarGrid = &rAIM.m_PolarGrid[0];
	CHUNK_GAHAIM_INFO::VirtualExample example;

	f32 ew0=(1-px)*(1-py); 
	idx=(iy+0)*CHUNK_GAHAIM_INFO::XGRID+(ix+0);
	example = pPolarGrid[idx];
	i0		= example.i0;	if (i0==0xff) return;
	i1		= example.i1;	if (i1==0xff) return;
	i2		= example.i2;	if (i2==0xff) return;
	i3		= example.i3;	if (i3==0xff) return;
	weights[i0]+=f32(example.v0)/f32(0x2000)*ew0;	
	weights[i1]+=f32(example.v1)/f32(0x2000)*ew0;	
	weights[i2]+=f32(example.v2)/f32(0x2000)*ew0;	
	weights[i3]+=f32(example.v3)/f32(0x2000)*ew0;	


	f32 ew1=px*(1-py);
	idx=(iy+0)*CHUNK_GAHAIM_INFO::XGRID+(ix+1);
	example = pPolarGrid[idx];
	i0		= example.i0;	if (i0==0xff) return;
	i1		= example.i1;	if (i1==0xff) return;
	i2		= example.i2;	if (i2==0xff) return;
	i3		= example.i3;	if (i3==0xff) return;
	weights[i0]+=f32(example.v0)/f32(0x2000)*ew1;
	weights[i1]+=f32(example.v1)/f32(0x2000)*ew1;
	weights[i2]+=f32(example.v2)/f32(0x2000)*ew1;
	weights[i3]+=f32(example.v3)/f32(0x2000)*ew1;

	f32 ew3=(1-px)*py;		 
	idx=(iy+1)*CHUNK_GAHAIM_INFO::XGRID+(ix+0);
	example = pPolarGrid[idx];
	i0		= example.i0;	if (i0==0xff) return;
	i1		= example.i1;	if (i1==0xff) return;
	i2		= example.i2;	if (i2==0xff) return;
	i3		= example.i3;	if (i3==0xff) return;
	weights[i0]+= f32(example.v0)/f32(0x2000)*ew3;	
	weights[i1]+= f32(example.v1)/f32(0x2000)*ew3;	
	weights[i2]+= f32(example.v2)/f32(0x2000)*ew3;	
	weights[i3]+= f32(example.v3)/f32(0x2000)*ew3;	

	f32 ew2=px*py;
	idx=(iy+1)*CHUNK_GAHAIM_INFO::XGRID+(ix+1);
	example = pPolarGrid[idx];
	i0		= example.i0;	if (i0==0xff) return;
	i1		= example.i1;	if (i1==0xff) return;
	i2		= example.i2;	if (i2==0xff) return;
	i3		= example.i3;	if (i3==0xff) return;
	weights[i0]+= f32(example.v0)/f32(0x2000)*ew2;
	weights[i1]+= f32(example.v1)/f32(0x2000)*ew2;
	weights[i2]+= f32(example.v2)/f32(0x2000)*ew2;
	weights[i3]+= f32(example.v3)/f32(0x2000)*ew2;

	f32 sum=0.0f;	for(uint32 i=0; i<numAimPosesCAF; i++) sum+=weights[i];
	assert( fabs(sum-1.0f)<0.001 );

	for (uint32 i=0; i<numAimPosesCAF; i++)
	{
		DynArray<Vec3> & rAimPosistions = rAIM.m_arrAimIKPosesAIM[i].m_arrPosition;

		const Quat *arrRotations = &rAIM.m_arrAimIKPosesAIM[i].m_arrRotation[0];		
		const Vec3 *arrPositions = 0;

		if (!rAimPosistions.empty())
			arrPositions = &rAimPosistions[0];

		f32 w=weights[i]*fIKBlend;
		if (w)
		{
			//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"Index: %d  weights: %f",i,weights[i] );	
			//	g_YLine+=16.0f;
			for (uint32 r=0; r<numRotJoints; r++)
			{
				int32 j	= rRot[r].m_nJointIdx;
				qtemp = arrRelPose[r];

				//	if (i==0)
				//	{
				//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"exist: %d  %f %f %f %f  joint-name: %s",(rAIM.m_nExist&(uint64(1)<<r))!=0,pMiddleRot[r].w,pMiddleRot[r].v.x,pMiddleRot[r].v.y,pMiddleRot[r].v.z,  pAimIK_Rot[r].m_strJointName );	
				//	g_YLine+=16.0f;
				//	}

				if (rAIM.m_nExist&(uint64(1)<<r))
				{
					qtemp.q = arrRotations[r];
					if (rRot[r].m_nAdditive) 
						qtemp.q = arrRelPose[r].q*(!pMiddleRot[r]*qtemp.q);     //additive blend

					int32 posIdx	= rRot[r].m_nPosIndex;
					if (posIdx>-1 && arrPositions)
					{
						qtemp.t = arrPositions[posIdx];
						if (rPos[posIdx].m_nAdditive)
							qtemp.t = arrRelPose[r].t-(pMiddlePos[r]-qtemp.t);     //additive blend
					}
				}
				jointsRelative[j].q += qtemp.q*w;
				jointsRelative[j].t += qtemp.t*w;
			}
		}
	}


	/*
	for (uint32 r=0; r<1; r++)
	{
	for (uint32 i=0; i<numAimPosesCAF; i++)
	{
	const Quat *arrRotations = &rAIM.m_arrAimIKPosesAIM[i].m_arrRotation[0];
	int32 j	= pAimIK_Rot[r].m_nJointIdx;
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"exist: %d  Quat(%f %f %f %f)  joint-name: %s",(rAIM.m_nExist&(uint64(1)<<r))!=0,arrRotations[r].w,arrRotations[r].v.x,arrRotations[r].v.y,arrRotations[r].v.z,  pAimIK_Rot[r].m_strJointName );	
	g_YLine+=16.0f;
	}
	g_YLine+=16.0f;
	}

	for (uint32 p=0; p<1; p++)
	{
	for (uint32 i=0; i<numAimPosesCAF; i++)
	{
	const Vec3 *arrPositions = 0;
	if (!rAIM.m_arrAimIKPosesAIM[i].m_arrPosition.empty())
	{
	arrPositions = &rAIM.m_arrAimIKPosesAIM[i].m_arrPosition[0];
	}
	int32 j	= pAimIK_Pos[p].m_nJointIdx;
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.4f, fTextColor, false,"exist: %d  Vec3(%f %f %f)  joint-name: %s",(rAIM.m_nExist&(uint64(1)<<p))!=0,arrPositions[p].x,arrPositions[p].y,arrPositions[p].z,  pAimIK_Pos[p].m_strJointName );	
	g_YLine+=16.0f;
	}
	g_YLine+=16.0f;
	} */

}

#if !(defined(XENON) || defined(PS3))
#pragma warning(push)
#pragma warning(disable : 6262) // debug stuff only (assume enough stack space)
void SDirectionalBlender::DebugVEGrid(const SAnimationPoseModiferParams& params, SDirInfo& rAimInfo, f32 fAimPoseWeight, f32 fIKBlend, int32 widx, SJointsAimIK_Rot* rRot,uint32 numRotJoints,  SJointsAimIK_Pos* rPos,uint32 numPosJoints)
{
	//VExampleInit ve;
	float fTextColor[4] = {1,0,0,1};
	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.2f, fTextColor, false,"fAimPoseWeight: %f",fAimPoseWeight );	
	g_YLine+=14.0f;
	if (fAimPoseWeight<0.01f)
		return;

	CCharInstance* pInstance = PoseModifierHelper::GetCharInstance(params);

	Matrix33 m33; m33.SetRotationX(-gf_PI/2);
	m33.m00=-m33.m00;	
	m33.m01=-m33.m01; 
	m33.m02=-m33.m02;


	for (f32 x=-gf_PI; x<=gf_PI+0.001f; x+=gf_PI/8 )
		g_pAuxGeom->DrawLine( m33*Vec3(x,-gf_PI/2,-0.001f),RGBA8(0x1f,0x0f,0x7f,0xff), m33*Vec3(x,gf_PI/2,-0.001f), RGBA8(0x1f,0x0f,0x7f,0xff) );
	for (f32 y=-gf_PI/2; y<=gf_PI/2+0.001f; y+=gf_PI/8 )
		g_pAuxGeom->DrawLine( m33*Vec3(-gf_PI,  y,-0.001f),RGBA8(0x1f,0x0f,0x7f,0xff), m33*Vec3(gf_PI,  y,-0.001f), RGBA8(0x1f,0x0f,0x7f,0xff) );

	CModelSkeleton* pModelSkeleton =	&pInstance->m_pModel->m_ModelSkeleton;
	uint32 numJoints = pModelSkeleton->m_poseData.GetJointCount();
	const CModelJoint* parrModelJoints = &pModelSkeleton->m_arrModelJoints[0];
	CAnimationSet* pAnimationSet =		&pInstance->m_pModel->m_AnimationSet;

	GlobalAnimationHeaderAIM& rAIM0 = g_AnimationManager.m_arrGlobalAIM[rAimInfo.m_nGlobalDirID0];

	QuadIndices arrQuat[255];
	uint32 numAimPoses=uint32(rAIM0.m_fTotalDuration/SECONDS_PER_TICK+1.1f);
	uint32 numQuats = rAIM0.Debug_AnnotateExamples2(numAimPoses, arrQuat);


	//--------------------------------------------------------------------------------
	//----    debug stuff
	//--------------------------------------------------------------------------------
	static Ang3 ang3 = Ang3(0,0,0);
	ang3+=Ang3(0.01f,0.02f,0.13f);
	AABB aabb1=AABB( Vec3(-0.01f,-0.01f,-0.01f), Vec3(+0.01f,+0.01f,+0.01f));
	AABB aabb2=AABB( Vec3(-0.02f,-0.02f,-0.02f), Vec3(+0.02f,+0.02f,+0.02f));
	OBB obb1; obb1.SetOBBfromAABB(Matrix33::CreateRotationXYZ(ang3),aabb1);
	OBB obb2; obb2.SetOBBfromAABB(Matrix33::CreateRotationXYZ(ang3),aabb2);

	QuatT arrRelPose0[256];
	QuatT arrAbsPose0[256];
	cryMemcpy( arrRelPose0, &pModelSkeleton->m_poseData.m_pJointsRelative[0], numJoints*sizeof(QuatT) );
	for (uint32 i=0; i<numJoints; i++) arrAbsPose0[i]=pModelSkeleton->m_poseData.m_pJointsAbsolute[i];

	QuatT arrRelPose1[256];
	QuatT arrAbsPose1[256];
	cryMemcpy( arrRelPose1, &pModelSkeleton->m_poseData.m_pJointsRelative[0], numJoints*sizeof(QuatT) );
	for (uint32 i=0; i<numJoints; i++) arrAbsPose1[i]=pModelSkeleton->m_poseData.m_pJointsAbsolute[i];

	QuatT arrRelPose2[256];
	QuatT arrAbsPose2[256];
	cryMemcpy( arrRelPose2, &pModelSkeleton->m_poseData.m_pJointsRelative[0], numJoints*sizeof(QuatT) );
	for (uint32 i=0; i<numJoints; i++) arrAbsPose2[i]=pModelSkeleton->m_poseData.m_pJointsAbsolute[i];

	QuatT arrRelPose3[256];
	QuatT arrAbsPose3[256];
	cryMemcpy( arrRelPose3, &pModelSkeleton->m_poseData.m_pJointsRelative[0], numJoints*sizeof(QuatT) );
	for (uint32 i=0; i<numJoints; i++) arrAbsPose3[i]=pModelSkeleton->m_poseData.m_pJointsAbsolute[i];

	QuatT arrRelPose[256];
	QuatT arrAbsPose[256];
	cryMemcpy( arrRelPose, &pModelSkeleton->m_poseData.m_pJointsRelative[0], numJoints*sizeof(QuatT) );
	for (uint32 i=0; i<numJoints; i++) arrAbsPose[i]=pModelSkeleton->m_poseData.m_pJointsAbsolute[i];

	static Ang3 ang = Ang3(0,0,0);
	ang+=Ang3(0.1f,0.2f,0.03f);
	AABB aabb=AABB( Vec3(-0.01f,-0.01f,-0.01f), Vec3(+0.01f,+0.01f,+0.01f));
	OBB obb; obb.SetOBBfromAABB(Matrix33::CreateRotationXYZ(ang),aabb);

	ColorB tricol = RGBA8(0x00,0xff,0x00,0xff);

	//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.2f, fTextColor, false,"rAIM0.m_MiddleAimPose: %f %f %f %f",rAIM0.m_MiddleAimPose.w,  rAIM0.m_MiddleAimPose.v.x,rAIM0.m_MiddleAimPose.v.y,rAIM0.m_MiddleAimPose.v.z );	
	//	g_YLine+=14.0f;

	for (uint32 aq=0; aq<numQuats; aq++)
	{
		int8 i0=arrQuat[aq].i0;
		int8 i1=arrQuat[aq].i1;
		int8 i2=arrQuat[aq].i2;
		int8 i3=arrQuat[aq].i3;

		rAIM0.Debug_Blend4AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, i0,i1,i2,i3, arrQuat[aq].w0,  arrRelPose0,arrAbsPose0 );
		Quat qt0=rAIM0.m_MiddleAimPose*arrAbsPose0[widx].q;
		Vec2 polar0=Vec2(rAIM0.Debug_PolarCoordinate(qt0));
		//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.2f, fTextColor, false,"polar0: %f %f",polar0.x,polar0.y );	
		//	g_YLine+=14.0f;

		rAIM0.Debug_Blend4AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, i0,i1,i2,i3, arrQuat[aq].w1,  arrRelPose1,arrAbsPose1  );
		Quat qt1=rAIM0.m_MiddleAimPose*arrAbsPose1[widx].q;
		Vec2 polar1=Vec2(rAIM0.Debug_PolarCoordinate(qt1));
		//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.2f, fTextColor, false,"polar1: %f %f",polar1.x,polar1.y );	
		//	g_YLine+=14.0f;

		rAIM0.Debug_Blend4AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, i0,i1,i2,i3, arrQuat[aq].w2,  arrRelPose2,arrAbsPose2 );
		Quat qt2=rAIM0.m_MiddleAimPose*arrAbsPose2[widx].q;
		Vec2 polar2=Vec2(rAIM0.Debug_PolarCoordinate(qt2));
		//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.2f, fTextColor, false,"polar2: %f %f",polar2.x,polar2.y );	
		//	g_YLine+=14.0f;

		rAIM0.Debug_Blend4AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, i0,i1,i2,i3, arrQuat[aq].w3,  arrRelPose3,arrAbsPose3  );
		Quat qt3=rAIM0.m_MiddleAimPose*arrAbsPose3[widx].q;
		Vec2 polar3=Vec2(rAIM0.Debug_PolarCoordinate(qt3));
		//	g_pIRenderer->Draw2dLabel( 1,g_YLine, 1.2f, fTextColor, false,"polar3: %f %f",polar3.x,polar3.y );	
		//	g_YLine+=24.0f;

		ColorB col2;
		f32 t=1.0;
		f32 angle0	=	acos_tpl(qt0|qt1);
		f32 step0		=	1.0f/(angle0*30.0f);
		Vec2 p0 = polar0;
		g_pAuxGeom->DrawOBB(obb,m33*Vec3(polar0),1,RGBA8(0x00,0x1f,0x00,0x00),eBBD_Extremes_Color_Encoded);
		g_pAuxGeom->DrawOBB(obb,m33*Vec3(polar1),1,RGBA8(0x00,0x1f,0x00,0x00),eBBD_Extremes_Color_Encoded);
		g_pAuxGeom->DrawOBB(obb,m33*Vec3(polar2),1,RGBA8(0x00,0x1f,0x00,0x00),eBBD_Extremes_Color_Encoded);
		g_pAuxGeom->DrawOBB(obb,m33*Vec3(polar3),1,RGBA8(0x00,0x1f,0x00,0x00),eBBD_Extremes_Color_Encoded);
		for (f32 i=step0; i<3.0f; i+=step0)
		{
			t=i;	if (i>0.999) t=1.0;
			rAIM0.Debug_NLerp2AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, arrRelPose0,arrRelPose1,t,arrAbsPose);
			Quat qt = rAIM0.m_MiddleAimPose*arrAbsPose[widx].q;
			Vec2 p1 = Vec2(rAIM0.Debug_PolarCoordinate(qt));
			col2.r=uint8(arrQuat[aq].col.r*fAimPoseWeight);
			col2.g=uint8(arrQuat[aq].col.g*fAimPoseWeight);
			col2.b=uint8(arrQuat[aq].col.b*fAimPoseWeight);
			g_pAuxGeom->DrawLine( m33*(p0+arrQuat[aq].height),col2, m33*(p1+arrQuat[aq].height),col2 );
			p0=p1;
			if (t==1.0)
				break;
		}

		f32 angle1	=	acos_tpl(qt1|qt2);
		f32 step1		=	1.0f/(angle1*30.0f);
		for (f32 i=step1; i<3.0f; i+=step1)
		{
			t=i;	if (i>0.999) t=1.0;
			rAIM0.Debug_NLerp2AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, arrRelPose1,arrRelPose2,t,arrAbsPose);
			Quat qt = rAIM0.m_MiddleAimPose*arrAbsPose[widx].q;
			Vec2 p1 = Vec2( rAIM0.Debug_PolarCoordinate(qt) );
			col2.r=uint8(arrQuat[aq].col.r*fAimPoseWeight);
			col2.g=uint8(arrQuat[aq].col.g*fAimPoseWeight);
			col2.b=uint8(arrQuat[aq].col.b*fAimPoseWeight);
			g_pAuxGeom->DrawLine( m33*(p0+arrQuat[aq].height),col2, m33*(p1+arrQuat[aq].height),col2 );
			p0=p1;
			if (t==1.0)
				break;
		}

		f32 angle2	=	acos_tpl(qt2|qt3);
		f32 step2		=	1.0f/(angle2*30.0f);
		for (f32 i=step2; i<3.0f; i+=step2)
		{
			t=i; if (i>0.999) t=1.0;
			rAIM0.Debug_NLerp2AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, arrRelPose2,arrRelPose3,t,arrAbsPose);
			Quat qt = rAIM0.m_MiddleAimPose*arrAbsPose[widx].q;
			Vec2 p1 = Vec2( rAIM0.Debug_PolarCoordinate(qt) );
			col2.r=uint8(arrQuat[aq].col.r*fAimPoseWeight);
			col2.g=uint8(arrQuat[aq].col.g*fAimPoseWeight);
			col2.b=uint8(arrQuat[aq].col.b*fAimPoseWeight);
			g_pAuxGeom->DrawLine( m33*(p0+arrQuat[aq].height),col2, m33*(p1+arrQuat[aq].height),col2 );
			p0=p1;
			if (t==1.0)
				break;
		}


		f32 angle3	=	acos_tpl(qt3|qt0);
		f32 step3		=	1.0f/(angle3*30.0f);
		for (f32 i=step3; i<3.0f; i+=step3)
		{
			t=i;	if (i>0.999) t=1.0;
			rAIM0.Debug_NLerp2AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, arrRelPose3,arrRelPose0,t,arrAbsPose);
			Quat qt = rAIM0.m_MiddleAimPose*arrAbsPose[widx].q;
			Vec2 p1 = Vec2( rAIM0.Debug_PolarCoordinate(qt) );
			col2.r=uint8(arrQuat[aq].col.r*fAimPoseWeight);
			col2.g=uint8(arrQuat[aq].col.g*fAimPoseWeight);
			col2.b=uint8(arrQuat[aq].col.b*fAimPoseWeight);
			g_pAuxGeom->DrawLine( m33*(p0+arrQuat[aq].height),col2, m33*(p1+arrQuat[aq].height),col2 );
			p0=p1;
			if (t==1.0)
				break;
		}
	}

	for (int32 y=0; y<CHUNK_GAHAIM_INFO::YGRID; y++)
	{
		for (int32 x=0; x<CHUNK_GAHAIM_INFO::XGRID; x++)
		{
			//	if (rAIM0.m_PolarGrid[y*XGRID+x].m_fSmalest<0.2f)
			{
				int i0 = rAIM0.m_PolarGrid[y*CHUNK_GAHAIM_INFO::XGRID+x].i0;
				int i1 = rAIM0.m_PolarGrid[y*CHUNK_GAHAIM_INFO::XGRID+x].i1;
				int i2 = rAIM0.m_PolarGrid[y*CHUNK_GAHAIM_INFO::XGRID+x].i2;
				int i3 = rAIM0.m_PolarGrid[y*CHUNK_GAHAIM_INFO::XGRID+x].i3;
				Vec4 w;
				w.x = f32(rAIM0.m_PolarGrid[y*CHUNK_GAHAIM_INFO::XGRID+x].v0)/f32(0x2000);
				w.y = f32(rAIM0.m_PolarGrid[y*CHUNK_GAHAIM_INFO::XGRID+x].v1)/f32(0x2000);
				w.z = f32(rAIM0.m_PolarGrid[y*CHUNK_GAHAIM_INFO::XGRID+x].v2)/f32(0x2000);
				w.w = f32(rAIM0.m_PolarGrid[y*CHUNK_GAHAIM_INFO::XGRID+x].v3)/f32(0x2000);
				rAIM0.Debug_Blend4AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, i0,i1,i2,i3, w, arrRelPose,arrAbsPose );
				Vec3 vPolarCoord=Vec2(rAIM0.Debug_PolarCoordinate(rAIM0.m_MiddleAimPose*arrAbsPose[widx].q));
				g_pAuxGeom->DrawOBB(obb1,m33*vPolarCoord,1,RGBA8(0xff,0xff,0xff,0x00),eBBD_Extremes_Color_Encoded);
			}
		}
	}

	size_t nHowManyVE = rAIM0.VE2.size();
	for (uint32 i=0; i<nHowManyVE; i++)
	{
		int32 i0	=rAIM0.VE2[i].i0;
		int32 i1	=rAIM0.VE2[i].i1;
		int32 i2	=rAIM0.VE2[i].i2;
		int32 i3	=rAIM0.VE2[i].i3;
		Vec2 polcor;
		polcor.x=rAIM0.VE2[i].polar.x;
		polcor.y=rAIM0.VE2[i].polar.y;

		Vec4 w;
		w.x		=rAIM0.VE2[i].w0;
		w.y		=rAIM0.VE2[i].w1;
		w.z		=rAIM0.VE2[i].w2;
		w.w		=rAIM0.VE2[i].w3;
		rAIM0.Debug_Blend4AimPose( pModelSkeleton,rRot,numRotJoints,rPos,numPosJoints, i0,i1,i2,i3, w, arrRelPose,arrAbsPose );
		Vec3 vPolarCoord=Vec2(rAIM0.Debug_PolarCoordinate(rAIM0.m_MiddleAimPose*arrAbsPose[widx].q));

		assert(fabs(polcor.x-polcor.x)<0.00001);
		assert(fabs(polcor.y-polcor.y)<0.00001);
		g_pAuxGeom->DrawOBB(obb1,m33*vPolarCoord,1,RGBA8(0x00,0xff,0xff,0x00),eBBD_Extremes_Color_Encoded);
	}
}
#pragma warning(pop)
#endif



